//-----------------------------------------------------------------------
//
//  Integrator Comms Control
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//  $File: IntegratorControl.cs $ $Revision: 1 $
//-----------------------------------------------------------------------

//using Microsoft.Ccr.Core;
//using Microsoft.Dss.Core;

using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Threading;
using ProMRDS.Robotics.Integrator;
using System.Collections;

using brick = ProMRDS.Robotics.GenericBrick.Proxy;

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

// Integrator Command
//
// Code		Parameter		Meaning
// Space/CR	            Stop
// A                    Turn Left Forwards1
// B                    Beep (Takes 200 ms to execute)
// D                    Turn Right Forwards1
// H        Pin (0-7)   High (Set pin to 1)
// I                    Input (Read all pins)
// L        Pin (0-7)   Low (Set pin to 0)
// O    xx (Two hex digits)   Output (Write to all pins) 
// NOTE: This command is dangerous because it sets the motors and also
// interferes with the operation of the Bluetooth module.
// P        Pin (0-7)   Pin Input
// S                    Drive Backward
// V                    Version (Displays version string)
// W                    Drive Forward
// X     Behavior       Execute Behavior
//          L           Back up, Rotate Left (90 degrees)
//          R           Back up, Rotate Right (90 degrees)
//          U           Back up, Do U-Turn (180 degrees)
//          NOTE: The turn angles are ONLY APPROXIMATE
//       Behavior xx    Execute Behavior with Parameter (in hex)
//          Axx         Anti-Clockwise (Rotate Left) xx units
//          Bxx         Backward xx units
//          Cxx         Clockwise (Rotate Right) xx units
//          Fxx         Forward xx units
//          NOTE: The units in all cases are ARBITRARY
// , or <               Rotate Left
// . or >               Rotate Right
// [ or {               Turn Left Backwards
// ] or }               Turn Right Backwards
// +                    Soft Reset
//
// Turn operations are performed by leaving one motor OFF and turning on
// the other motor in the appropriate direction so the robot pivots around
// the stationary wheel. There are four ways this can be done. Two move
// the robot forward and the other two move the robot backward.
// Rotate (spin) operations are performed by running the motors in opposite
// directions. There are only two possibilities: Clockwise (right) or
// Anti-Clockwise (left).

namespace ProMRDS.Robotics.Integrator.Comms
{
    /// <summary>
    /// All concurrency is done by calling service. Assume this is not thread safe
    /// </summary>
    internal class IntegratorControl
    {

        SerialPort _serialPort;
//        IntegratorOperations _integPort = null;
        brick.GenericBrickOperations _integPort = null;

        // Port for sending commands to
        public Port<Command> CommandPort = new Port<Command>();

        // Use our own dispatcher because we block on I/O
        Dispatcher _dispatcher;
        DispatcherQueue _taskQueue;
        
        // Response from the Integrator to a command packet
        byte[] _response = new byte[80];
        int _responseCount = 0;

        // IR values have to change more than this before a
        // notification is triggered
        //const int IR_NOTIFICATION_THRESHOLD = 5;
        // Values higher than this are going to be seen as a
        // "bump" of the sensor
        //const int IR_STATE_THRESHOLD = 32;

        // Anything over this threshold turns the motors on
        const double POWER_THRESHOLD = 0.25;

        bool Connected = false;

        // Modified for CF version
        public int Delay = 100;

        public int MessagesSentCounter = 0;
        public int ErrorCounter = 0;

        public const int IR_COUNT = 1;
//        public const int SWITCH_COUNT = 4;

        public const int LED_COUNT = 2;

        // Array of IR sensors
        private int[] priorIRValues = new int[IR_COUNT];
        public int[] IRValues = new int[IR_COUNT];

        private int[] LEDPins = { (int)Pins.LED1, (int)Pins.LED2 };

//        private int priorSwitches = 0;
//        public int Switches;

        #region Constructor and Destructor

        public IntegratorControl(brick.GenericBrickOperations integratorPort)
        {
            int i;

            _integPort = integratorPort;

            // Note that the prior values are initialised so that
            // there will be a notification on the very first update
            /*
            for (i=0; i<IR_COUNT; i++)
            {
                priorIRValues[i] = -IR_NOTIFICATION_THRESHOLD;
                IRValues[i] = 0;
            }
             */
            // Explicitly create a Dispatcher so we can control the pool size
            _dispatcher = new Dispatcher(3, "Comms Pool");
            _taskQueue = new DispatcherQueue("Comms Queue", _dispatcher);

            // Wait one time for an InternalDrivePower command
            Arbiter.Activate(_taskQueue, Arbiter.ReceiveWithIterator<Command>(false, CommandPort, CommandHandler));

        }

        ~IntegratorControl()
        {
            Close();
        }
        #endregion

        #region Serial Port Control

        /// <summary>
        /// Connect to the Robot Brick
        /// </summary>
        /// <param name="cfg"></param>
        /// <param name="errorMessage"></param>
        /// <returns>True for success, False for Failure. Also errorMessage is set if there is a problem</returns>
        public bool Connect(brick.CommsConfig cfg, out string errorMessage)
        {
            if (cfg.SerialPort <= 0)
            {
                errorMessage = "Serial port is not configured";
                return false;
            }

            if (Connected)
                Close();

            try
            {
                string ComPort = "COM" + cfg.SerialPort.ToString();

                // Create a new serial port connection
                // Note that the baud rate and data format are hard-coded
                _serialPort = new SerialPort(ComPort, 9600, Parity.None, 8, StopBits.One);

                // Added for CF version
                // In terms of the baud rate, this is an eternity
                _serialPort.WriteTimeout = cfg.Timeout;
                _serialPort.ReadTimeout = cfg.Timeout;

                Delay = cfg.Delay;

                // Attempt to open the connection
                _serialPort.Open();

                // If we reach here, it was successful
                _serialPort.DiscardInBuffer();
                errorMessage = string.Empty;
                Connected = true;
                return true;
            }
            catch (Exception ex)
            {
                // Error occurred in opening the connection so report it
                errorMessage = string.Format("Error connecting to robot on COM{0}: {1}",
                                    cfg.SerialPort, ex.Message);
                return false;
            }
        }

        /// <summary>
        /// Close Serial Port
        /// </summary>
        public void Close()
        {
            if (!Connected)
                return;
            Connected = false;

            // Wait a little while to make sure we don't kill an
            // operation in progress
            Thread.Sleep(100);
            if (_serialPort != null && _serialPort.IsOpen)
            {
                _serialPort.Close();
                _serialPort = null;
            }

        }

        // IMPORTANT NOTE:
        // This routine runs synchronously and should only be used when the
        // service starts up.
        // The process for connecting is simple:
        // 1. Send a CR to ensure that we are synchronized with the robot
        // 2. Send a Version command
        // 3. Get the response and check that it is valid
        // 4. Parse the version number and return it
        public bool WaitForConnect(brick.Firmware firmware)
        {
            // Send a command to "wake up" the robot
            byte[] buf = new byte[1];
            buf[0] = 13;        // CR
            // Should get an "OK" response, but we don't check
            SendPacket(buf);

            // Send a command to display the Integrator OS version
            buf[0] = (byte)'V';
            SendPacket(buf);

            // No response?
            if (_responseCount <= 0)
                return false;

            // Extract the version string
            // NOTE: Quite often the first couple of characters are lost!
            // This is why we send a CR first
            StringBuilder sb = new StringBuilder();
            for (int i = 0; i < _responseCount; i++)
                sb.Append((char)_response[i]);

            string version = sb.ToString();

            // Return the version info
            firmware.VersionString = version;

            // Extract the version number (if possible)
            int offset = version.LastIndexOf(".");
            int begin;
            int minor = 0;
            if (offset >= 0)
            {
                begin = offset - 1;
                for (int i = offset + 1; i < version.Length; i++)
                {
                    if (version[i] < '0' || version[i] > '9')
                        break;
                    minor = minor * 10 + (version[i] - (int)'0');
                }
            }
            else
                begin = version.Length - 1;

            int major = 0;
            for (; begin >= 0; begin--)
            {
                if (version[begin] < '0' || version[begin] > '9')
                    break;
            }
            begin++;
            if ((offset - begin) > 0)
            {
                for (int i = begin; i < offset; i++)
                {
                    if (version[i] < '0' || version[i] > '9')
                        break;
                    major = major * 10 + (version[i] - (int)'0');
                }
            }
            firmware.MajorVersion = major;
            firmware.MinorVersion = minor;

            // Flashes both front LEDs and sounds a tone
            //SetLEDs(0x0C);
            //PlayToneRequest(3000, 100);
            SetPin((int)Pins.Buzzer, true);
            SetPin((int)Pins.LED1, true);
            SetPin((int)Pins.LED2, true);
            Thread.Sleep(500);
            SetPin((int)Pins.Buzzer, false);
            SetPin((int)Pins.LED1, false);
            SetPin((int)Pins.LED2, false);
            //SetLEDs(0);

            return true;
        }

        #endregion

        // Perform the necessary transformation from the MRDS
        // power range of -1.0 to +1.0 to the robot's range
        // which is on +/- 1 because it can't do speeds.
        int TransformPower(double? power)
        {
            int p;

            if (power == null)
                return 0;

            p = 0;
            if (power >= POWER_THRESHOLD)
                p = 1;
            if (power <= -POWER_THRESHOLD)
                p = -1;

            return p;
        }

        /// <summary>
        /// Set Power to Motors
        /// </summary>
        /// <param name="left">Range is -1.0 to 1.0</param>
        /// <param name="right">Range is -1.0 to 1.0</param>
        public Port<byte[]> SetPower(double? left, double? right)
        {
            int powerLeft, powerRight;
            int temp;

            if ((left != null && (left < -1.0 || left > 1.0)) 
                || ((right != null) && (right < -1.0 || right > 1.0))) 
                throw new ArgumentOutOfRangeException("Invalid Motor Power");

            if (left != null)
                temp = TransformPower(left);
            else
                temp = 0;

            powerLeft = temp;

            if (right != null)
                temp = TransformPower(right);
            else
                temp = 0;

            powerRight = temp;

            return SendPower(powerLeft, powerRight);
        }

        public Port<byte[]> Poll()
        {
            Command cmd = new Command();

            cmd.CommandString = new byte[1];
            cmd.CommandString[0] = (byte)'I';
            CommandPort.Post(cmd);
            return cmd.ResponsePort;
        }

        #region Control

        /// <summary>
        /// Set Pin
        /// </summary>
        /// <param name="pin">Pin on the Brick</param>
        /// <param name="state">True/False = On/Off or High/Low</param>
        /// <returns>Port for the response from the robot</returns>
        public Port<byte[]> SetPin(int pin, bool state)
        {
            byte[] on = { (byte)'H', (byte)'0' };
            byte[] off = { (byte)'L', (byte)'0' };

            Command cmd = new Command();
            if (state)
            {
                on[1] = (byte)((byte)'0' + pin);
                cmd.CommandString = on;
            }
            else
            {
                off[1] = (byte)((byte)'0' + pin);
                cmd.CommandString = off;
            }

            CommandPort.Post(cmd);

            return cmd.ResponsePort;
        }

        /*
        // Turns the speaker on for the specified duration
        private IEnumerator<ITask> SpeakerTone(int ms)
        {
            byte[] on = { (byte)'H', (byte)'7' };
            byte[] off = { (byte)'L', (byte)'7' };

//            SpeakerState(true);


            Command cmd = new Command();
            cmd.CommandString = on;
            CommandPort.Post(cmd);

            DateTime now = DateTime.Now;

            // Wait for the response
            yield return (Arbiter.Receive(false, cmd.ResponsePort,
                delegate(byte[] resp)
                {
                }
            ));

            // Calculate the delay so far
            int delay = (int)DateTime.Now.Subtract(now).TotalMilliseconds;

            delay = ms - delay;

            if (delay > 0)
                yield return (Arbiter.Receive(false, Timeout);

            //SendPacket(on);

            // Set up a timer for the specified interval
            //            SpawnIterator<int>(ms, SpeakerTimer);

            // Wait for the duration
            Thread.Sleep(ms);

            // Turn off the sound now
//            SpeakerState(false);
        }
         */
        /*
        private void SpeakerState(bool status)
        {
            // Speaker ON command
            byte[] buf = { (byte)'H', (byte)',', (byte)'1' };

            if (status)
            {
                SendPacket(buf);
            }
            else
            {
                // Change ON to OFF
                buf[2] = (byte)'0';
                SendPacket(buf);
            }
        }
        */
//        private IEnumerable<ITask> SpeakerTimer(int duration)
//        {
//            // Wait the specified period
//            yield return Arbiter.Receive(false, TimeoutPort(duration), delegate(DateTime now) { SpeakerState(false); });
//
//        }


        // Current settings of the LEDs
        private static byte LEDs = 0;

        public void SetLEDs(int states)
        {
            int mask = 1;
            for (int i = 0; i < LED_COUNT; i++)
            {
                // Don't care about the response
                SetPin(LEDPins[i], ((mask & states) != 0));
                mask = mask << 1;
            }
            LEDs = (byte) states;
        }

        #endregion

        #region Navigation


        // Motor Settings
        //
        // Motors on the Integrator cannot be speed controlled
        // due to hardware limitations -- Only one PWM channel,
        // and bit-banging serial I/O with no timeout support.
        //
        // Each motor has three states: Stop, Forward, Reverse.
        // Therefore each motor requires two pins (4 binary states).
        // So 4 output pins are dedicated to the motors.
        //
        // Motors settings are as follows (total of 16 combinations):
        //
        // Right   Left     Val  Cmd   Action
        // Fwd Rev Fwd Rev
        //  0   1   2   3
        //  H   L   H   L   5    W      Forward
        //  L   H   L   H   10   S      Reverse
        //  H   L   L   L   1    A      Turn Left
        //  L   L   H   L   4    D      Turn Right
        //  H   L   L   H   6    <      Spin Left
        //  L   H   H   L   9    >      Spin Right
        //  L   L   L   L   0    SP/CR  Stop
        //
        // Two others are also possible,
        // but perhaps not obvious, by
        // running one motor backwards:
        //  L   H   L   L   2    [      Back Left
        //  L   L   L   H   8    ]      Back Right
        //
        // Some combinations are illegal
        // because they would require a motor
        // to go in both directions at once!
        //  H   H   H   H   15
        //  H   H   H   L   7
        //  H   H   L   H   11
        //  H   H   L   L   3
        //  H   L   H   H   13
        //  L   H   H   H   14
        //  L   L   H   H   12

        private static byte[] motorCommands =
        {
            13,         // L  L  L  L   0   Stop
            (byte)'A',  // H  L  L  L   1   Turn Left
            (byte)'[',  // L  H  L  L   2   Back Left
            0,          // H  H  L  L   3
            (byte)'D',  // L  L  H  L   4   Turn Right
            (byte)'W',  // H  L  H  L   5   Forward
            (byte)'<',  // H  L  L  H   6   Spin Left
            0,          // H  H  H  L   7
            (byte)']',  // L  L  L  H   8   Back Right
            (byte)'>',  // L  H  H  L   9   Spin Right
            (byte)'S',  // L  H  L  H   10  Reverse
            0,          // H  H  L  H   11
            0,          // L  L  H  H   12
            0,          // H  L  H  H   13
            0,          // L  H  H  H   14
            0           // H  H  H  H   15
        };
 
        // Send the wheel speeds to the Integrator
        // The speeds should only be > 0, = 0 or < 0
        private Port<byte[]> SendPower(int left, int right)
        {
            byte[] buf = new byte[1];
            int index = 0;

            // Check the order!
            /*
            if (right > 0)
                index = 1;
            else if (right < 0)
                index = 2;

            if (left > 0)
                index += 4;
            else if (left < 0)
                index += 8;
            */
            if (right > 0)
                index = 4;
            else if (right < 0)
                index = 8;

            if (left > 0)
                index += 1;
            else if (left < 0)
                index += 2;

            buf[0] = motorCommands[index];

            Command cmd = new Command(buf);
            CommandPort.Post(cmd);
            return cmd.ResponsePort;
            //SendPacket(buf);
        }

        #endregion

        #region Communication

        /// <summary>
        /// Serial Port Command Handler
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        /// NOTE: This handler executes serial commands which block the thread
        private IEnumerator<ITask> CommandHandler(Command cmd)
        {
            byte[] response = new byte[0];

            if (cmd.CommandString == null || cmd.CommandString.Length == 0)
            {
                // Pathological case -- no command!
                // Somebody sent us an empty packet, so send back an empty response
                cmd.ResponsePort.Post(response);
            }
            else
            {
                // Send the packet and wait for a response
                SendPacket(cmd.CommandString);

                // Got a response?
                if (_responseCount > 0)
                {
                    byte[] resp = new byte[_responseCount];
                    for (int i = 0; i < _responseCount; i++)
                        resp[i] = _response[i];
                    cmd.ResponsePort.Post(resp);
                }
                else
                    cmd.ResponsePort.Post(response);
            }

            // Wait one time for a new Command
            Arbiter.Activate(_taskQueue, Arbiter.ReceiveWithIterator<Command>(false, CommandPort, CommandHandler));

            yield break;
        }


        // Carriage Return
        static char[] CR = { '\r' };

        private DateTime _lastPacketTime;

        // SendPacket()
        // Baud Rate is 9600 which is approximately 960 chrs/sec
        // which is about a millisecond per character.
        // NOTE: SendPacket() operates SYNCHRONOUSLY, although it does
        // have timeouts to prevent it from hanging forever.
        private bool SendPacket(byte[] buf)
        {
            TimeSpan timeDiff;
            int msDiff;
            bool ok = true;

            // Always check! There could be outstanding messages when the
            // connection is closed.
            if (!_serialPort.IsOpen)
                return false;

            // Check that this packet is not too close to the last one.
            // It is ESSENTIAL that there is a delay after sending
            // a command because the Integrator cannot process the
            // commands very quickly and will start to lose data!
            timeDiff = (DateTime.Now - _lastPacketTime);
            msDiff = timeDiff.Milliseconds;
            if (msDiff < Delay)
            {
                // Wait for the difference in time
                Thread.Sleep(Delay - msDiff);
            }

            _responseCount = 0;

            try
            {
                int i;
                // Throw away any left-over data in the input buffer
                _serialPort.DiscardInBuffer();

                // Output the packet one byte at a time to give the
                // Integrator time to digest the characters.
                // Otherwise it gets a data overrun and locks up!
                for (i = 0; i < buf.Length; i++)
                {
                    _serialPort.Write(buf, i, 1);
                    Thread.Sleep(10);
                }
            }
            catch (Exception ex)
            {
                ErrorCounter++;
                Console.WriteLine("Comms Write Error on command " + 
                    (char)buf[0] + " (Error count " + ErrorCounter + ")");
                Console.WriteLine("Exception: " + ex.Message);
                // Try to re-synch
                _serialPort.Write(CR, 0, 1);
                ok = false;
            }
            finally
            {
                _lastPacketTime = DateTime.Now;
            }
            if (!ok)
                return false;

            ok = true;
            try
            {
                int i, val;

                i = 0;
                while ((val = _serialPort.ReadByte()) != '\r')
                {
                    _response[i] = (byte)val;
                    i++;
                }
                _responseCount = i;

                // Throw away the rest of the response (only a LF)
                _serialPort.DiscardInBuffer();
            }
            catch (Exception ex)
            {
                ErrorCounter++;
                Console.WriteLine("Comms Read Error on command " +
                    (char)buf[0] + " (Error count " + ErrorCounter + ")");
                Console.WriteLine("Exception: " + ex.Message);
                // Send a CR to try to re-synch
                _serialPort.Write(CR, 0, 1);
                ok = false;
            }
            finally
            {
                _lastPacketTime = DateTime.Now;
            }
            if (!ok)
                return false;

            MessagesSentCounter++;
            return true;
        }

        #endregion
    }

    /// <summary>
    /// Serial Command
    /// </summary>
    /// <remarks>Used internally to send packets to the serial port</remarks>
    public class Command
    {
        public byte[] CommandString;
        public Port<byte[]> ResponsePort;

        public Command()
        {
            CommandString = null;
            // Make sure that there is always a response port
            ResponsePort = new Port<byte[]>();
        }
        public Command(byte[] buf)
        {
            CommandString = buf;
            ResponsePort = new Port<byte[]>();
        }

    }

}
